/*
 * caosensornode.cpp
 *
 *
 *  Created on: Oct 8, 2011
 *  Author: Mario H. Voorsluys
 *	email: forcaeluz@gmail.com
 *
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Library General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, visit:
 * http://www.gnu.org/copyleft/gpl.html
 */

#include <iostream>
#include <cstdlib>

#include "caosensornode.h"

CaoSensorNode::CaoSensorNode():node_handle("") {
	init();
}

CaoSensorNode::~CaoSensorNode() {
	node_handle.shutdown();
}

void CaoSensorNode::init(){
	//Set addresses
	sonar1_addr = 0xE0;
	sonar2_addr = 0xE2;
	sonar3_addr = 0xE4;
	/*
	 * Open the connection with the serial port. The file descriptot in my laptop is
	 * /dev/ttyUSB0
	 * In other computers it will probably something similar. It could be /dev/ttyS0 for example.
	 */
	if(communicator.open_connection((char*)"/dev/ttyUSB0", 2000)){
		cout << "Connection open" << endl;
	}
	/*
	 * If connection doesn't fail, there will be a device id. (Not very usefull, only for check). Could also ask for software version,
	 * to asure a minimum version is running inside the device, but for now, not really important.
	 */
	ROS_INFO("Connected to CAO board, id: %d", communicator.deviceId());

	//Tell ros we have publishers, one for each sensor.
	sonar1_publisher = node_handle.advertise<std_msgs::Int32>("sonar1", MESSAGE_QUEUE_SIZE);
	sonar2_publisher = node_handle.advertise<std_msgs::Int32>("sonar2", MESSAGE_QUEUE_SIZE);
	sonar3_publisher = node_handle.advertise<std_msgs::Int32>("sonar3", MESSAGE_QUEUE_SIZE);

	sharp1_publisher = node_handle.advertise<std_msgs::Int32>("sharp1", MESSAGE_QUEUE_SIZE);
	sharp2_publisher = node_handle.advertise<std_msgs::Int32>("sharp2", MESSAGE_QUEUE_SIZE);
	sharp3_publisher = node_handle.advertise<std_msgs::Int32>("sharp3", MESSAGE_QUEUE_SIZE);
	sharp4_publisher = node_handle.advertise<std_msgs::Int32>("sharp4", MESSAGE_QUEUE_SIZE);


}

void CaoSensorNode::spin(){
	/*
	 * No more than 4 Hz, limited mainly by the timing of the sonars.
	 * Each one needs 70 ms to range, and some extra time for communication. With 3, this would be 210ms seconds only to range.
	 * Adding communication to it, and the other sensors, which also have their ADC convertion time, the total time could be 0,250 seconds.
	 */
	ros::Rate loopRate(4);
	while(ros::ok()){
		std_msgs::Int32 msg; //My message
		int sonar_value = communicator.getTWIData(sonar1_addr); //get the data from the sensor.
		msg.data = sonar_value; // data into message
		sonar1_publisher.publish(msg); //publish the data.
		if(sonar_value < 0){//If value is ok, just INFO it, if not, WARN it.
			ROS_WARN("Sonar 0x%X distance reading error.", sonar1_addr);
		}else{
			ROS_INFO("Sonar 0x%X distance: %dcm",sonar1_addr, sonar_value);
		}
		//Same, for the other sonars.
		sonar_value = communicator.getTWIData(sonar2_addr);
		msg.data = sonar_value;
		sonar2_publisher.publish(msg);
		if(sonar_value < 0){
			ROS_WARN("Sonar 0x%X distance reading error.", sonar2_addr);
		}else{
			ROS_INFO("Sonar 0x%X distance: %dcm",sonar2_addr, sonar_value);
		}
		sonar_value = communicator.getTWIData(sonar3_addr);
		msg.data = sonar_value;
		sonar3_publisher.publish(msg);
		if(sonar_value < 0){
			ROS_WARN("Sonar 0x%X distance reading error.", sonar3_addr);
		}else{
			ROS_INFO("Sonar 0x%X distance: %dcm",sonar3_addr, sonar_value);
		}
		loopRate.sleep();
	}
}
